#pragma once

#include <fstream>
#include <iomanip>
#include <iostream>
#include <map>
#include <memory>
#include <unordered_map>

// glog
#include "glog/logging.h"

// Eigen
#include "Eigen/Core"
#include "Eigen/Dense"

// Opencv
#include "opencv2/opencv.hpp"

// rosbag
#include "rosbag/bag.h"
#include "rosbag/view.h"

// ros msgs
#include "cv_bridge/cv_bridge.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/image_encodings.h"

// vio
#include "vio/vio_pipeline.h"

namespace vio_bag_sim {
// msg type
enum class MsgType {
  IMU,
  LEFT_IMG,
  RIGHT_IMG,
};
struct MsgTypeIdx {
  MsgType type;
  size_t idx;
};

class VIOBagSim {
 public:
  VIOBagSim(const std::string& bag_path) : bag_path_(bag_path) {}
  // API
  void Initialize();
  void Step();
  void Terminate();

 protected:
  void LoadMsgsFromBag();

 private:
  template <typename T>
  void GetMsgsFromBag(const rosbag::Bag& bag,
                      const std::string& topic_name,
                      std::vector<T>* msgs_ptr);

  template <typename T>
  void AddMsgsToMap(const MsgType& type,
                    const std::vector<T>& msgs,
                    std::multimap<double, MsgTypeIdx>* map_ptr);

 private:
  std::string bag_path_{""};
  // all msgs index
  std::multimap<double, MsgTypeIdx> bag_msgs_map_;
  // msgs
  std::vector<sensor_msgs::Imu> imus_;
  std::vector<sensor_msgs::Image> left_imgs_;
  std::vector<sensor_msgs::Image> right_imgs_;

  // vio pipeline
  vio::VIOPipeline vio_pipeline_;
};
}  // namespace vio_bag_sim
